#!/usr/bin/env python

import roslib; roslib.load_manifest('slu_manager')
import rospy
from std_msgs.msg import String
import tf
import traceback

def transform_listener_to_pose_map(tl, source_transform="odom_combined"):
    """
    Converts a TransformerROS to a map containing the current pose of
    the robot.  Keys are the transform name, values are a tuple of
    translation and rotation.

    This dictionary contains only python primitive types and can be
    repr'd and eval'd without any dependencies on ROS.
    """
    data = {}
    for key in ["r_gripper_palm_link", "l_gripper_palm_link", #"l_gripper_tool_frame"
                "base_link"]:
        try:
            (trans,rot) = tl.lookupTransform('/odom_combined', key, 
                                         rospy.Time(0))
            data[key] = trans, rot
        except tf.LookupException:
            pass
        except tf.ConnectivityException:
            pass
        except tf.ExtrapolationException:
            pass

    return data

def talker():
    rospy.init_node('slu_manager')

    tl = tf.TransformListener()
    pub = rospy.Publisher('/blocknlp/slu_pose', String)

    rate = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        
        try:
            data = transform_listener_to_pose_map(tl)
            rospy.loginfo("publishing transforms")
            pub.publish(String(repr(data)))

        except:
            traceback.print_exc()
        rate.sleep()

def main():
    try:
        talker()
    except rospy.ROSInterruptException: pass

if __name__ == '__main__':
    main()
